#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include "gazebo_msgs/ModelStates.h"
#include "gazebo_msgs/WorldState.h"

namespace gazebo
{

  class ROSControl : public ModelPlugin
  {
  public:
    // 用于加载Gazebo插件的函数，用于初始化模型、ROS节点、订阅者和事件连接。
    void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {
      //std::cout<<"rr"<<std::endl;
      // 将父模型指针赋值给成员变量
      this->model = _parent;

      // 获取模型的名称并赋值给成员变量
      this->model_name = _parent->GetName();

      // 检查ROS是否已初始化
      if (!ros::isInitialized())
      {
        // 初始化ROS节点
        int argc = 0;
        char **argv = NULL;
        ros::init(argc, argv, "obstacle_control", ros::init_options::NoSigintHandler);
      }

      // 创建一个新的ROS节点句柄
      this->rosNode.reset(new ros::NodeHandle());

      // 订阅/obstacles_velocity话题，并将消息传递给obstacleCallback回调函数
      this->rosSub = this->rosNode->subscribe("/obstacles_velocity", 1000, &ROSControl::obstacleCallback, this);

      // 将OnUpdate函数绑定到世界更新事件
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
          std::bind(&ROSControl::OnUpdate, this));
    }

  public:
    // 用于在每次仿真世界更新时执行的回调函数。该函数设置模型的线速度和角速度，从而控制模型在仿真环境中的运动。
    void OnUpdate()
    {
      // 设置模型的线速度
      this->model->SetLinearVel(ignition::math::Vector3d(this->linear_x, this->linear_y, this->linear_z));


      // 设置模型的角速度，这里设置为零向量
      this->model->SetAngularVel(ignition::math::Vector3d(0, 0, 0));
    }

    // 用于处理从/obstacles_velocity话题接收到的消息。该函数检查接收到的消息中的模型名称列表，并将与当前模型匹配的模型的线速度提取出来，更新为成员变量的值，以便在仿真中使用这些速度值。
    void obstacleCallback(const gazebo_msgs::WorldState::ConstPtr &message)
    {

      // 获取当前模型的名称
      auto name = this->model_name;

      // 获取消息中模型名称列表的大小
      int num = message->name.size();

      // 初始化模型名称和速度的迭代器
      auto iter_name = message->name.begin();
      auto iter_twist = message->twist.begin();

      // 遍历所有模型名称
      for (int i = 0; i < num; i++)
      {
        // 检查当前模型名称是否与接收到的模型名称匹配
        if (name == *iter_name)
        {
          // 如果匹配，将接收到的线速度更新到成员变量中
          this->linear_x = (*iter_twist).linear.x;
          this->linear_y = (*iter_twist).linear.y;
          this->linear_z = (*iter_twist).linear.z;
        }
        else
        {
          // 如果不匹配，移动到下一个模型名称和速度
          iter_name++;
          iter_twist++;
        }
      }
    }

  private:
    physics::ModelPtr model;

  private:
    event::ConnectionPtr updateConnection;

  private:
    std::unique_ptr<ros::NodeHandle> rosNode;

  private:
    float linear_x{};

  private:
    float linear_y{};

  private:
    float linear_z{};

  private:
    ros::Subscriber rosSub;

  private:
    std::string model_name;
  };

  GZ_REGISTER_MODEL_PLUGIN(ROSControl)
}
